#include "rgbd_xtion.h"

RGBD_xtion::RGBD_xtion()
{
}

RGBD_xtion::~RGBD_xtion()
{
    this->interface->stop();
}

void RGBD_xtion::init(){
    update_cloud = 0;
    update_image = 0;
    update_depth = 0;
    cloud_sence = pcl::PointCloud<pcl::PointXYZRGBA>::Ptr(new pcl::PointCloud<pcl::PointXYZRGBA>);
    run();
}

void RGBD_xtion::cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud)
{
  if (update_cloud == 1){
    pcl::copyPointCloud(*cloud, *cloud_sence);
    update_cloud = 0;
  }
 }
void RGBD_xtion::image_cb_ (const boost::shared_ptr<pcl::io::OpenNI2Grabber::Image>& img)
  {
      if (update_image == 1){
      getFrame (img);
      update_image = 0;
    }
  }

void RGBD_xtion::depth_cb_ (const boost::shared_ptr<pcl::io::OpenNI2Grabber::DepthImage> &img)
  {
      if (update_depth == 1){
          cv::Mat frameRGB=cv::Mat(img->getHeight(),img->getWidth(),CV_32FC1);
          img->fillDepthImage(frameRGB.cols,frameRGB.rows,(float*)frameRGB.data);
          depth_image = frameRGB;
      update_depth = 0;
    }
  }

void RGBD_xtion::getFrame (const boost::shared_ptr<pcl::io::OpenNI2Grabber::Image> &img)
  {
    cv::Mat frameRGB=cv::Mat(img->getHeight(),img->getWidth(),CV_8UC3);
    img->fillRGB(frameRGB.cols,frameRGB.rows,frameRGB.data,frameRGB.step);
    cv::cvtColor(frameRGB,rgb_image,CV_RGB2BGR);

  }

void RGBD_xtion::run()
{
  interface = new pcl::io::OpenNI2Grabber();
      // create a new grabber for OpenNI devices

  // make callback function from member function
  boost::function<void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f =
    boost::bind (&RGBD_xtion::cloud_cb_, this, _1);

  boost::function<void (const boost::shared_ptr<pcl::io::OpenNI2Grabber::Image>&)>  g =
   boost::bind (&RGBD_xtion::image_cb_, this, _1);
  boost::function<void (const boost::shared_ptr<pcl::io::OpenNI2Grabber::DepthImage>&)>  h =
   boost::bind (&RGBD_xtion::depth_cb_, this, _1);
  // connect callback function for desired signal. In this case its a point cloud with color values
  interface->registerCallback (f);
  interface->registerCallback (g);
  interface->registerCallback (h);
  interface->start ();

}

void RGBD_xtion::fetchXYZ(pcl::PointCloud<pcl::PointXYZ> *XYZ){
    update_cloud = 1;
    while (update_cloud == 1){
        usleep(100);
    }
    pcl::copyPointCloud(*(this->cloud_sence), *XYZ);
}

void RGBD_xtion::fetchXYZRGB(pcl::PointCloud<pcl::PointXYZRGB> *XYZRGB){
    update_cloud = 1;
    while (update_cloud == 1){
        usleep(100);
    }
    pcl::copyPointCloud(*(this->cloud_sence), *XYZRGB);
}

void RGBD_xtion::fetchColorImage(cv::Mat &image){
    update_image = 1;
    while (update_image == 1){
        usleep(100);
    }
    image = this->rgb_image.clone();
}

void RGBD_xtion::fetchDepthImage(cv::Mat &image){
    update_depth = 1;
    while (update_depth == 1){
        usleep(100);
    }
    image = this->depth_image.clone();
}
